#include <stdint.h>
#include <stdio.h>
#include "mpu6050.h"

#include "portmacro.h"
#include "sdkconfig.h"
#include "esp_log.h"
#include "driver/i2c_master.h"

#define SCL_IO_PIN  15
#define SDA_IO_PIN  16
#define PORT_NUMBER -1

#define MPU6050_ADDR        0x68    // I2C 地址（AD0接地）
#define I2C_MASTER_FREQ_HZ  400000  // I2C 时钟频率（400kHz）

#define ACCEL_CONFIG_VAL    0x00    // 0x00=±2g, 0x08=±4g, 0x10=±8g, 0x18=±16g
#define GYRO_CONFIG_VAL     0x00    // 0x00=±250°/s, 0x08=±500°/s, 0x10=±1000°/s, 0x18=±2000°/s

//****************************************
// 定义MPU6050内部地址
//****************************************
#define SMPLRT_DIV   0x19 //陀螺仪采样率，典型值：0x07(125Hz)
#define CONFIG       0x1A //低通滤波频率，典型值：0x06(5Hz)
#define GYRO_CONFIG  0x1B //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)
#define ACCEL_CONFIG 0x1C //加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz)
#define ACCEL_XOUT_H 0x3B
#define ACCEL_XOUT_L 0x3C
#define ACCEL_YOUT_H 0x3D
#define ACCEL_YOUT_L 0x3E
#define ACCEL_ZOUT_H 0x3F
#define ACCEL_ZOUT_L 0x40
#define TEMP_OUT_H   0x41
#define TEMP_OUT_L   0x42
#define GYRO_XOUT_H  0x43
#define GYRO_XOUT_L  0x44
#define GYRO_YOUT_H  0x45
#define GYRO_YOUT_L  0x46
#define GYRO_ZOUT_H  0x47
#define GYRO_ZOUT_L  0x48
#define PWR_MGMT_1   0x6B //电源管理，典型值：0x00(正常启用)
#define WHO_AM_I     0x75 //IIC地址寄存器(默认数值0x68，只读)
#define SlaveAddress 0xD0 //IIC写入时的地址字节数据，+1为读取

i2c_master_dev_handle_t g_i2cDev;

// 写入寄存器
static esp_err_t mpu6050_register_write(uint8_t reg, uint8_t value) {
    uint8_t buf[2] = {reg, value};
    return i2c_master_transmit(g_i2cDev, buf, 2, portMAX_DELAY);
}

// 读取寄存器
static esp_err_t mpu6050_register_read(uint8_t reg, uint8_t *buf, size_t len) {
    return i2c_master_transmit_receive(g_i2cDev, &reg, 1, buf, len, portMAX_DELAY);
}

static float mpu6050_accel_convert(int16_t accel) {
    switch (ACCEL_CONFIG_VAL) {
        case 0x00:
            return accel / 16384.0;
        case 0x08:
            return accel / 8192.0;
        case 0x10:
            return accel / 4096.0;
        case 0x18:
            return accel / 2048.0;
    }
    return 0;
}

static float mpu6050_gyro_convert(int16_t gyro) {
    switch (GYRO_CONFIG_VAL) {
        case 0x00:
            return gyro / 131.0;
        case 0x08:
            return gyro / 65.5;
        case 0x10:
            return gyro / 32.8;
        case 0x18:
            return gyro / 16.4;
    }
    return 0;
}

// 读取原始数据
esp_err_t mpu6050_read(mpu6050_data_t *data) {
    uint8_t buf[14];
    ESP_ERROR_CHECK(mpu6050_register_read(0x3B, buf, sizeof(buf)));

    data->accel_x = mpu6050_accel_convert(((uint16_t)buf[0] << 8) | buf[1]);
    data->accel_y = mpu6050_accel_convert(((uint16_t)buf[2] << 8) | buf[3]);
    data->accel_z = mpu6050_accel_convert(((uint16_t)buf[4] << 8) | buf[5]);
    // data->temp    = (((uint16_t)buf[6] << 8) | buf[7]) / 340.0 + 36.53;
    data->gyro_x  = mpu6050_gyro_convert(((uint16_t)buf[8] << 8) | buf[9]);
    data->gyro_y  = mpu6050_gyro_convert(((uint16_t)buf[10] << 8) | buf[11]);
    data->gyro_z  = mpu6050_gyro_convert(((uint16_t)buf[12] << 8) | buf[13]);

    return ESP_OK;
}

static void mpu6050_i2cMasterInit(void)
{
    i2c_master_bus_config_t i2c_bus_config = {
        .clk_source = I2C_CLK_SRC_DEFAULT,
        .i2c_port = PORT_NUMBER,
        .scl_io_num = SCL_IO_PIN,
        .sda_io_num = SDA_IO_PIN,
        .glitch_ignore_cnt = 7,
        .flags.enable_internal_pullup = true
    };
    i2c_master_bus_handle_t bus_handle;

    ESP_ERROR_CHECK(i2c_new_master_bus(&i2c_bus_config, &bus_handle));

    i2c_device_config_t i2c_dev_conf = {
        .scl_speed_hz   = I2C_MASTER_FREQ_HZ,
        .device_address = MPU6050_ADDR,
    };

    ESP_ERROR_CHECK(i2c_master_bus_add_device(bus_handle, &i2c_dev_conf, &g_i2cDev));
}

void mpu6050_init(void)
{
    mpu6050_i2cMasterInit();

    mpu6050_register_write(PWR_MGMT_1, 0x00);   //解除休眠状态
    mpu6050_register_write(SMPLRT_DIV, 0x07);   //设置采样率
    mpu6050_register_write(CONFIG, 0x06);       //设置采样率, 5Hz
    mpu6050_register_write(GYRO_CONFIG, 0x00);  //设置陀螺仪, 0x00=±250°/s, 0x08=±500°/s, 0x10=±1000°/s, 0x18=±2000°/s
    mpu6050_register_write(ACCEL_CONFIG, 0x00); //设置加速度, 0x00=±2g, 0x08=±4g, 0x10=±8g, 0x18=±16g
}
